import sys
import rospy
import moveit_commander
from geometry_msgs.msg import Pose
import numpy as np
from tf.transformations import quaternion_from_euler, quaternion_multiply

# Initialize MoveIt Commander and ROS node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("moveit_grasp", anonymous=True)

# Initialize robot and planning groups
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander("panda_arm")
gripper_group = moveit_commander.MoveGroupCommander("panda_hand")


# Move to start position
# Define the start position
start_pose = Pose()
start_pose.position.x = 0.3  # Replace with your desired x-coordinate
start_pose.position.y = 0.0  # Replace with your desired y-coordinate
start_pose.position.z = 0.5  # Replace with your desired z-coordinate
start_pose.orientation.x = -0.9225889390271658  # Replace with your desired orientation (quaternion)
start_pose.orientation.y = 0.3857815910973857
start_pose.orientation.z = 0.0006983880319421323
start_pose.orientation.w = 0.0013137005956108379

# Define the eye-to-hand offset
eyehand_offset = [0.04, -0.01] # 相机-手爪偏移

# Move to the start position
arm.set_pose_target(start_pose)
plan = arm.plan()
if plan:
    arm.go(wait=True)
else:
    print("Failed to plan to the start position.")

# Open the gripper
gripper_group.set_named_target("open")
gripper_group.go(wait=True)



# Get current pose
current_pose = arm.get_current_pose().pose
print("Current Pose:", current_pose)

# Use run_inference() from local inference.py
folder = "/home/robot1/JuliusWorkplace/Tongji_Intelligent_Systems_Lab/OBB_yolov8"
if folder not in sys.path:
    sys.path.insert(0, folder)

from inference import run_inference

# Run inference and print structured results
results = run_inference()
print("----------results----------")

# (index, X, Y, Z, angle_deg, u, v)
for item in results:
    print(item)

# Keep for later cells (e.g., motion planning)
targets = results

theta_deg = results[0][4]  # Example: get angle from first result



if theta_deg >90:
    theta_deg = theta_deg - 180
print("Theta (degrees):", theta_deg)
X = results[0][1]  # Example: get X from first result
Y = results[0][2]  # Example: get Y from first result



# Extract the current orientation as a quaternion
current_orientation = current_pose.orientation
current_quaternion = [current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w]

# Define the rotation angle in degrees and convert to radians

theta_rad = np.deg2rad(theta_deg)  # Convert to radians




# Generate a quaternion for the rotation around the Z-axis (yaw)
rotation_quaternion = quaternion_from_euler(0, 0, theta_rad)  # Roll=0, Pitch=0, Yaw=theta_rad

# Combine the current orientation with the rotation
new_quaternion = quaternion_multiply(current_quaternion, rotation_quaternion)



sys.path.append('/home/robot1/JuliusWorkplace/Tongji_Intelligent_Systems_Lab/Moveit')

from cal import rotate_clockwise_np
# Define the target pose
target_pose = Pose()

rot_offset= rotate_clockwise_np(eyehand_offset, theta_rad)
print("Rotated offset:", rot_offset)


target_pose.position.x = current_pose.position.x - Y + eyehand_offset[0]
target_pose.position.y = current_pose.position.y - X + eyehand_offset[1]
target_pose.position.z = current_pose.position.z 





target_pose.orientation.x = new_quaternion[0]
target_pose.orientation.y = new_quaternion[1]
target_pose.orientation.z = new_quaternion[2]
target_pose.orientation.w = new_quaternion[3]

print("-------------------------------------------")
print("Target position with rotation:", target_pose)

arm.set_pose_target(target_pose)
plan = arm.plan()

if plan:
    print("Plan found, executing...")
    arm.go(wait=True)
else:
    print("Planning failed!")
print("--------------------------------------------")
current_pose = arm.get_current_pose().pose
print("Current Pose:", current_pose)
arm.stop()
arm.clear_pose_targets()

# Move down to grasp

target_pose.position.z = 0.105

arm.set_pose_target(target_pose)
plan = arm.plan()
if plan:
    print("Plan found, executing...")
    arm.go(wait=True)
else:
    print("Planning failed!")

print("--------------------------------------------")
current_pose = arm.get_current_pose().pose
print("Current Pose:", current_pose)

#close the gripper
gripper_group.set_named_target("close")
gripper_group.go(wait=True)

# Move back to the start position
arm.set_pose_target(start_pose)
plan = arm.plan()
if plan:
    arm.go(wait=True)
else:
    print("Failed to plan to the start position.")
# Open the gripper
gripper_group.set_named_target("open")
gripper_group.go(wait=True)

# Shut down MoveIt cleanly
arm.stop()
arm.clear_pose_targets()
moveit_commander.roscpp_shutdown()